import pandas as pd
import matplotlib.pyplot as plt

# 读取 CSV
df = pd.read_csv("data.csv")

# 时间轴
t = range(len(df))

# ---------- 绘制 IMU 数据 ----------
plt.figure(figsize=(16, 14))

# IMU0 gyr
plt.subplot(4, 1, 1)
plt.plot(t, df['imu0_gyr_x'], label='imu0_gyr_x')
plt.plot(t, df['imu0_gyr_y'], label='imu0_gyr_y')
plt.plot(t, df['imu0_gyr_z'], label='imu0_gyr_z')
plt.title("IMU0 Gyroscope")
plt.xlabel("Frame")
plt.ylabel("rad/s")
plt.legend()
plt.grid(True)

# IMU0 acc
plt.subplot(4, 1, 2)
plt.plot(t, df['imu0_acc_x'], label='imu0_acc_x')
plt.plot(t, df['imu0_acc_y'], label='imu0_acc_y')
plt.plot(t, df['imu0_acc_z'], label='imu0_acc_z')
plt.title("IMU0 Accelerometer")
plt.xlabel("Frame")
plt.ylabel("m/s²")
plt.legend()
plt.grid(True)

# IMU1 gyr
plt.subplot(4, 1, 3)
plt.plot(t, df['imu1_gyr_x'], label='imu1_gyr_x')
plt.plot(t, df['imu1_gyr_y'], label='imu1_gyr_y')
plt.plot(t, df['imu1_gyr_z'], label='imu1_gyr_z')
plt.title("IMU1 Gyroscope")
plt.xlabel("Frame")
plt.ylabel("rad/s")
plt.legend()
plt.grid(True)

# IMU1 acc
plt.subplot(4, 1, 4)
plt.plot(t, df['imu1_acc_x'], label='imu1_acc_x')
plt.plot(t, df['imu1_acc_y'], label='imu1_acc_y')
plt.plot(t, df['imu1_acc_z'], label='imu1_acc_z')
plt.title("IMU1 Accelerometer")
plt.xlabel("Frame")
plt.ylabel("m/s²")
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

# ---------- 绘制激光测距 ----------
plt.figure(figsize=(16, 6))
plt.plot(t, df['laser_left'], label='laser_left')
plt.plot(t, df['laser_right'], label='laser_right')
plt.title("Laser Distance")
plt.xlabel("Frame")
plt.ylabel("m")
plt.legend()
plt.grid(True)
plt.show()

# ---------- 绘制编码器数据 ----------
plt.figure(figsize=(16, 8))
coder_cols = [c for c in df.columns if 'coder' in c]
for col in coder_cols:
    plt.plot(t, df[col], label=col)
# plt.plot(t, coder_cols[0])
# plt.plot(t, coder_cols[1])

plt.title("Encoder Data")
plt.xlabel("Frame")
plt.ylabel("rad")  # 假设编码器单位是 rad
plt.legend(ncol=5)
plt.grid(True)
plt.show()
